#!/usr/bin/env python3

import rospy
import random
import math
import tf
from geometry_msgs.msg import Pose, Point
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState

# 巡航点列表
waypoints = [
    (5, -3, 0), 
    (0,-3,0),
    (0,0,0),
    (3,2,0),
    (5,1,0),
    (-3,-3,0),
    (3,-2,0)
]

def move_model_to_point(model_name, x, y, z):
    rospy.wait_for_service('/gazebo/set_model_state')
    try:
        # 设定位置
        set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        state = ModelState()
        state.model_name = model_name
        state.pose.position.x = x
        state.pose.position.y = y
        state.pose.position.z = z
        state.pose.orientation.w = 1  # 无旋转
        set_state(state)
        
        # 发布从map到enemy_block的tf
        global tf_broadcaster
        tf_broadcaster.sendTransform(
            (x+5, y-3, z),  # 扫描得到的地图和物理地图之间有一个固定的偏移
            (0, 0, 0, 1),  # 无旋转
            rospy.Time.now(),
            model_name,
            "map"
        )
        
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s", e)

def interpolate_points(start, end, steps):
    """在起点和终点之间插值"""
    points = []
    for i in range(steps + 1):
        x = start[0] + (end[0] - start[0]) * (i / steps)
        y = start[1] + (end[1] - start[1]) * (i / steps)
        z = start[2] + (end[2] - start[2]) * (i / steps)
        points.append((x, y, z))
    return points

def cruise():
    log_info = False
    
    rospy.init_node('enemy_block_cruise', anonymous=True)
    rate = rospy.Rate(10)  # 控制移动速度（10 Hz）

    current_point = waypoints[0]  # 初始位置
    move_model_to_point('enemy_block', *current_point)  # 设置初始位置

    while not rospy.is_shutdown():
        # 随机选择一个巡航点
        target_point = random.choice(waypoints)
        if log_info:
            rospy.loginfo("Moving from %s to %s", current_point, target_point)

        # 插值生成路径点
        steps = 50  # 插值点数
        path = interpolate_points(current_point, target_point, steps)

        # 逐步移动
        for point in path:
            move_model_to_point('enemy_block', *point)
            rate.sleep()

        # 更新当前点
        current_point = target_point

if __name__ == '__main__':
    tf_broadcaster = tf.TransformBroadcaster()
    try:
        cruise()
    except rospy.ROSInterruptException:
        pass